#include <avr/io.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <stddef.h>

#include "vs1053_driver.h"

#include "debug.h"

// Inline function to increase performace, while also increasing code readibilty
#define VS_WRITE_ENDFILLBYTE() do { while(!(vsPORT.IN & vsDREQ)); vsPORT.OUT &= (~vsDCS); vsSPI.DATA = vsEndFillByte; while(!(vsSPI.STATUS & SPI_IF_bm)); vsPORT.OUT |= vsDCS; } while(0)

void vsInit(void)
{
	// Enable SCK,MOSI,CS,DCS,Reset as outputs - MISO,DREQ
	vsPORT.DIR = vsSCK | vsSI | vsCS | vsDCS | vsReset;

	// Set Reset high (it's inverted logic) to allow vs to boot
	vsPORT.OUT &= ~vsReset;

	// Initialize SPI on XMega at 500KHz
	vsSetCTRL(VS_SPI_SCK_LOWSPEED);

	// Make sure CS and DCS are not active
	vsPORT.OUT = vsCS | vsDCS;

	vsPORT.OUT |= vsReset;

	// Wait for DREQ to go to 1 -> Per application notes sect. 5
	while(!(vsPORT.IN & vsDREQ));

}

void vsSoftReset()
{
	// Set the RESET bit in mode register
	uint16_t mode = _BV(SM_SDINEW) | _BV(SM_LAYER12) | _BV(SM_RESET) | 0x0804;

	vsWriteRegister(VS_MODE,mode>>8,mode&0xFF);

	// Make sure CS and DCS are not set
	vsPORT.OUT |= vsCS | vsDCS;

	// Wait for DREQ to go to 1 -> Per application notes sect. 5
	while(!(vsPORT.IN & vsDREQ));
}

void vsHardReset()
{
	// Set reset pin low (XRESET is Active low)
	vsPORT.OUT &= ~vsReset;

	// Make sure CS and DCS are not set
	vsPORT.OUT |= vsCS | vsDCS;

	// Release Reset
	vsPORT.OUT |= vsReset;

	// Wait for DREQ to go to 1 -> Per application notes sect. 5
	while(!(vsPORT.IN & vsDREQ));

	vsSetCTRL(VS_SPI_SCK_LOWSPEED);

	vsConfigureDevice();
}

void vsConfigureDevice(void)
{
	uint16_t vsMode;
	uint16_t vsStatus;
	uint16_t vsClock;

	// Send garabage to clear the ports for send/rec
	vsWrite(0x00);

	// Read in the vs1053's MODE, STATUS, and CLOCKF registers
	vsMode		= vsReadRegister(VS_MODE);
	vsStatus	= vsReadRegister(VS_STATUS);
	vsClock		= vsReadRegister(VS_CLOCKF);

	// Load var with VS_MODE register mode values
	vsMode = _BV(SM_SDINEW) | _BV(SM_LAYER12);

	// Write new mode settings to vs1053
	vsWriteRegister(VS_MODE,(vsMode>>8)&0xFF, (vsMode&0xFF));

	// Write 0x8800 to VS_CLOCKF -> sets new multipled CLKI to 55.296MHz. SPI SCK can be no greater than (CLKI/7)=7.899 MHz
	vsWriteRegister(VS_CLOCKF,VS_CLOCKF_HIGHSPEED >> 8, VS_CLOCKF_HIGHSPEED & 0xFF );

	//Read back value. Note that the change in speed does not take effect for some time after modifying VS_CLOCKF
	vsClock = vsReadRegister(VS_CLOCKF);

	//if (vsClock != VS_CLOCKF_HIGHSPEED)
	//	printf("Error setting CLKI:   C: %x\r\n",vsClock);

	//else
	//	printf("Clock Set Correctly: C: %x\r\n",vsClock);

	// Wait for the vs1053 to finish its Clock configuration routines
	while(!(vsPORT.IN & vsDREQ));

	// Set SPI SCK Clock to 32MHz/8 = 4 MHz
	vsSetCTRL(VS_SPI_SCK_HIGHSPEED);

	// Verify we can talk to the vs1053 after updating the speed of SCK
	vsClock = vsReadRegister(VS_CLOCKF);

	//printf("High speed SPI Working: VS_CLOCKF=%X, SPI CLK_DIV=%X\r\n",vsClock, VS_SPI_SCK_HIGHSPEED);

	// Set default volume level
	vsSetVol(VOL_DEFAULT);

	// Make sure control lines are not set
	vsPORT.OUT |= (vsCS);
	vsPORT.OUT |= (vsDCS);

}

void vsSetCTRL(uint8_t clkDiv)
{
	//Make sure control lines are not active
	vsPORT.OUT |= (vsCS);
	vsPORT.OUT |= (vsDCS);

	// Temporarily turn off SPI port functionality so we can modify it's params
	vsSPI.CTRL = 0;

	//Write the new params to the CTRL port
	vsSPI.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc | clkDiv;

}

void vsSetVol(uint8_t volume)
{
	vsWriteRegister(VS_VOL, volume, volume);
}

void vsWriteRegister(enum VS_REGISTER reg, uint8_t highB, uint8_t lowB)
{
	// Wait until vs is ready to recieve data/commands
	while(!(vsPORT.IN & vsDREQ));

	// Send Start of Transfer (Set CS Low: Active Low)
	vsPORT.OUT &= ~(vsCS);

	// Tell the vs1053 this is a write command
	vsWriteMultiByte(VS_WRITE_REGISTER);

	// Write the vale of the register we want to write to
	vsWriteMultiByte(reg);

	// Write the high byte
	vsWriteMultiByte(highB);

	// Write the low byte
	vsWriteMultiByte(lowB);

	// Wait until vs completes operation
	while (!(vsPORT.IN & vsDREQ));

	// Release CS
	vsPORT.OUT |= vsCS;
}

uint16_t vsReadRegister(enum VS_REGISTER reg)
{
	uint16_t ret;

	// Wait for VS1053 to be ready to recieve instructions
	while(!(vsPORT.IN & vsDREQ));

	// Signal incoming command
	vsPORT.OUT &=~(vsCS);

	// Write Read Command
	vsWriteMultiByte(VS_READ_REGISTER);

	// Write the reg val we are interested in
	vsWriteMultiByte(reg);

	// Clear SPI so we can recieve byte
	vsWrite(0x00);

	ret = (vsSPI.DATA << 8) & 0xFF00;

	// Double check that VS1053 is ready to send next byte
	while(!(vsPORT.IN & vsDREQ));

	// Clear SPI so we can recieve byte
	vsWrite(0x00);

	ret |= (vsSPI.DATA&0x00FF);

	// Wait for VS1053 is finished executing internal instructions
	while(!(vsPORT.IN & vsDREQ));

	// Release the CS line
	PORTD.OUT |= vsCS;

	return ret;
}

void vsWriteMultiByte(uint8_t data)
{
	// Ensure VS1053 is not in the middle of executing a command
	while(!(vsPORT.IN & vsDREQ));

	//Begin byte transmission
	vsSPI.DATA = data;

	// Wait for transfer to complete
	while(!(vsSPI.STATUS & SPI_IF_bm));
}

void vsWrite(uint8_t data)
{
	// Wait for vs1053 to be ready for commands/data
	while(!(vsPORT.IN & vsDREQ));

	// Activate CS line
	vsPORT.OUT &= ~(vsCS);

	//Begin byte transmission
	vsSPI.DATA = data;

	// Wait for transfer to complete
	while(!(vsSPI.STATUS & SPI_IF_bm));

	// Release CS line
	vsPORT.OUT |= vsCS;
}

void vsWriteData(uint8_t *data)
{
	// Wait for the vs1053 to be ready for commands/data
	while(!(vsPORT.IN & vsDREQ));

	// Activate DCS (Data Control Select)
	vsPORT.OUT &= ~(vsDCS);

	// Write the data
	vsSPI.DATA = *data;

	// Wait for send to complete
	while(!(vsSPI.STATUS & SPI_IF_bm));

	// Release the Data Select Line
	vsPORT.OUT |= vsDCS;
}

uint8_t vsStopPlayBack()
{
	uint16_t i;
	uint16_t vsEndFillByte;
	vsEndFillByte = vsReadRegister(VS_MODE) | (1<<SM_CANCEL);

	// Try to put vs1053 in cancel playback mode
	vsWriteRegister(VS_MODE,(vsEndFillByte>>8) & (0xFF),vsEndFillByte&0xFF);
	vsWriteData(0);

	// Wait until vs1053 confirms cancel playback mode
	for (i=1,vsEndFillByte=0xffff; i<2048;i++)
	{
		if (i%32 == 0)
		{
			// If vs1053 cancels playback successfully, clear vsEndFillByte
			if (vsReadRegister(VS_MODE) & (1<<SM_CANCEL))
			{
				vsEndFillByte = 0;
				break;
			}
		}
	}
	// If vs1053 cancled playback sucessfully get and transmit vsEndFillByte 2053 times (per vs1053 documentation)
	if (!vsEndFillByte)
	{
		// Get vsEndFillByte
		vsWriteRegister(VS_WRAMADDR,0x1E, 0x06);
		vsEndFillByte = vsReadRegister(VS_WRAM) & 0x00FF;

		// Write it 2053 times
		for (i = 0; i < 2053; i++)
			VS_WRITE_ENDFILLBYTE();

		return 1;
	}
	// Else Perform soft reset (per vs1053 documentation)
	vsSoftReset();
	return 0;
}